#include "MovePrecompiled.h"
#include "EyeImage.h"

namespace Eye
{
	EyeImage::EyeImage(int w, int h, PBYTE data)
		:w(w),h(h),data(data)
	{
		ballColor=0;
		lastPos = Vector2(-1,-1);
	}


	EyeImage::~EyeImage(void)
	{
		if (ballColor)
			delete ballColor;
	}

	void EyeImage::setPixel(Vector2 pos, ColorRgb col)
	{
		if(pos.x>=w || pos.y>=h || pos.x<0 || pos.y<0)
			throw new MoveOutOfImageRangeException;
		data[pos.y*w*4+pos.x*4+0]=col.b; // B
		data[pos.y*w*4+pos.x*4+1]=col.g; // G
		data[pos.y*w*4+pos.x*4+2]=col.r; // R
	}

	ColorRgb EyeImage::getPixel(Vector2 pos)
	{
		ColorRgb col;
		if(pos.x>=w || pos.y>=h || pos.x<0 || pos.y<0)
		{
			throw new MoveOutOfImageRangeException;
		}
		col.b=data[pos.y*w*4+pos.x*4+0]; // B
		col.g=data[pos.y*w*4+pos.x*4+1]; // G
		col.r=data[pos.y*w*4+pos.x*4+2]; // R
		return col;
	}

	void EyeImage::drawCircle(Vector2 center, int r, ColorRgb col)
	{
		Vector2 pix;
		float d; //detail
		for(d=0; d<=2*Move::PI; d+=0.01f) //you can play with the value to be added to d
		{
			pix.x=(int)(center.x+sin(d)*r);
			pix.y=(int)(center.y+sin(d+(Move::PI/2))*r);
			if(!(pix.x>=w || pix.y>=h || pix.x<0 || pix.y<0))
				setPixel(pix,col);

		}
	}

	void EyeImage::RegisterBall()
	{
		if (ballColor)
			delete ballColor;
		ColorRgb middle=getPixel(Vector2(w/2,h/2));
		ballColor=new ColorHsv(middle);
	}

	PBYTE EyeImage::getData()
	{
		return data;
	}

	bool EyeImage::findBall(float &posX, float &posY, float &size)
	{
		if (!ballColor)
			return false;
		Vector2 currentPos;
		ColorRgb pixel;
		if (lastPos.x>=0 && pixelMatches(lastPos))
		{
			currentPos=lastPos;
			while (!pixelMatches(Vector2(currentPos.x-1,currentPos.y)))
			{
				currentPos.x--;
			}
		}
		else if (!combImage(currentPos))
		{
			lastPos=Vector2(-1,-1);
			return false;
		}

		findContour(currentPos);


		if (ballSize>15 && pixelMatches(Vector2((int)ballX,(int)ballY)))
		{
			//update ball color
			if (ballColor)
				delete ballColor;
			ballColor=new ColorHsv(getPixel(Vector2((int)ballX,(int)ballY)));
		}
		posX=ballX;
		posY=ballY;
		size=ballSize;

		if (size<2.0f)
			return false;

		return true;
	}

	void EyeImage::findContour(Vector2 init)
	{
		ballContour.clear();
		Vector2 current;
		if (!checkIfContour(init))
			return;
		ballContour.push_back(init);
		setPixel(init,ColorRgb(255,0,0));
		current=findNeighbor(init);
		if (current.x==-1)
			return;

		Vector2 min, max;
		min=max=current;
		while(current!=init)
		{
			ballContour.push_back(current);
			setPixel(current,ColorRgb(255,0,0));
			if (current.x<min.x) min.x=current.x;
			if (current.y<min.y) min.y=current.y;
			if (current.x>max.x) max.x=current.x;
			if (current.y>max.y) max.y=current.y;
			current = findNeighbor(current);
			if (current.x==-1)
			{
				while ( !ballContour.empty() )
				{
					Vector2 cnt = ballContour.back();
					ballContour.pop_back();
					current = findNeighbor(cnt);
					if (current.x!=-1)
						break;
				}
			}
			if (current.x==-1)
				break;
			if (ballContour.size()>2500)
				break;
		}
		ballSize=0.5f*(float)(max.x-min.x)+0.5f*(float)(max.y-min.y);
		ballX=0.5f*(float)(max.x+min.x);
		ballY=0.5f*(float)(max.y+min.y);
	}

	Vector2 EyeImage::findNeighbor(Vector2 pos)
	{
		Vector2 search;

		search=Vector2(pos.x+1,pos.y+1);
		if (checkIfContour(search))
			return search;
		search=Vector2(pos.x+1,pos.y);
		if (checkIfContour(search))
			return search;
		search=Vector2(pos.x+1,pos.y-1);
		if (checkIfContour(search))
			return search;
		search=Vector2(pos.x,pos.y-1);
		if (checkIfContour(search))
			return search;
		search=Vector2(pos.x-1,pos.y-1);
		if (checkIfContour(search))
			return search;
		search=Vector2(pos.x-1,pos.y);
		if (checkIfContour(search))
			return search;
		search=Vector2(pos.x-1,pos.y+1);
		if (checkIfContour(search))
			return search;
		search=Vector2(pos.x,pos.y+1);
		if (checkIfContour(search))
			return search;

		return Vector2(-1,-1);
	}

	bool EyeImage::checkIfContour(Vector2 pos)
	{
		if(pos.x>=w || pos.y>=h || pos.x<0 || pos.y<0)
			return false;
		ColorRgb pixel;
		if (!pixelMatches(pos))
			return false;
		if (pixelAlreadyFound(pos))
			return false;
		if (!pixelMatches(Vector2(pos.x-1,pos.y)))
			return true;
		if (!pixelMatches(Vector2(pos.x+1,pos.y)))
			return true;
		if (!pixelMatches(Vector2(pos.x,pos.y-1)))
			return true;
		if (!pixelMatches(Vector2(pos.x,pos.y+1)))
			return true;
		return false;
	}

	bool EyeImage::combImage(Vector2& found)
	{
		float searchGap=(float)h/2.0f;
		if (searchRow((int)searchGap,found))
			return true;

		float searchY;
		while (searchGap>4)
		{
			searchY=searchGap/2;
			while ((int)searchY<h)
			{
				if (searchRow((int)searchY,found))
					return true;
				searchY+=searchGap;
			}
			searchGap/=2;
		}
		return false;
	}

	bool EyeImage::searchRow(int y, Vector2& found)
	{
		//search in row
		for (int x=0;x<w;++x)
		{
			Vector2 currentPos=Vector2(x,y);
			ColorRgb pCol=getPixel(currentPos);
			if (colorMatches(&pCol))
			{
				if (checkBallAtPosition(currentPos))
				{
					found=currentPos;
					return true;
				}
			}
		}
		return false;
	}

	bool EyeImage::checkBallAtPosition(Vector2 pos)
	{
		return true;
	}

	bool EyeImage::colorMatches(ColorRgb *pixelColor)
	{
		if (pixelColor->r==255 && pixelColor->g==0 && pixelColor->b==0)
			return true;

		ColorHsv pixelHsv=ColorHsv(*pixelColor);

		//only for debug
		float x = ballColor->h;

		if (pixelHsv.v>0.7 && pixelHsv.h>ballColor->h-20 && pixelHsv.h<ballColor->h+20)
			return true;

		return false;
	}

	bool EyeImage::pixelMatches(Vector2 pos)
	{
		if(pos.x>=w || pos.y>=h || pos.x<0 || pos.y<0)
			return false;
		ColorRgb pixel;
		pixel=getPixel(pos);
		return colorMatches(&pixel);
	}

	bool EyeImage::pixelAlreadyFound(Vector2 pos)
	{
		if(pos.x>=w || pos.y>=h || pos.x<0 || pos.y<0)
			return false;
		ColorRgb pixel;
		pixel=getPixel(pos);
		if (pixel.r==255 && pixel.g==0 && pixel.b==0)
			return true;
		return false;
	}
}